

#ifndef ROBOT_DEFINES_H
#define ROBOT_DEFINES_H


struct cvec4d {
	double x;   // Position x,y,z
	double y;
	double z;
	double phi; // angle from the vector from this point back to the origin upward
              // towards the Z axis
  
  cvec4d() {
    clear();
  }
  
  cvec4d(double a_x, double a_y, double a_z, double a_phi) {
    x = a_x;
    y = a_y;
    z = a_z;
    phi = a_phi;
  }
  
  // Copy Constructor (create new object):
  cvec4d(cvec4d* vec) {
    x = vec->x;
    y = vec->y;
    z = vec->z;
    phi = vec->phi;
  }
  
  // Copy Function (set existing to same as other):
  void copy(cvec4d* vec) {
    x = vec->x;
    y = vec->y;
    z = vec->z;
    phi = vec->phi;
  }
  
  double get(uint32_t i) {
    switch(i) {
      case 0: return x;
      case 1: return y;
      case 2: return z;
      case 3: return phi;
      default: assert(0);
    }
  }
  
  void set(uint32_t i, double val) {
    switch(i) {
      case 0: x = val; return;
      case 1: y = val; return;
      case 2: z = val; return;
      case 3: phi = val; return;
      default: assert(0);
    }
  }
  
  void set(double a_x, double a_y, double a_z, double a_phi) {
    x = a_x;
    y = a_y;
    z = a_z;
    phi = a_phi;
  }
  
	void clear() {
		x = 0.0;
		y = 0.0;
		z = 0.0;
		phi = 0.0;
	}	
  
  void print() {
    printf("[%.2f, %.2f, %.2f, %.2f]", x, y, z, phi);
  }
  
  void println() {
    print();
    printf("\n");
  }
};


struct jvec4d {
	double t1;    // angles between joints w-1,1-2,2-3,3-4   (w is world ref)
	double t2;    // t1 to t4 in radians
	double t3;
	double t4;
  
  jvec4d() {
    clear();
  }
  
  jvec4d(double a_t1, double a_t2, double a_t3, double a_t4) {
    t1 = a_t1;
    t2 = a_t2;
    t3 = a_t3;
    t4 = a_t4;
  }
  
  // Copy Constructor (create new object):
  jvec4d(jvec4d* vec) {
    t1 = vec->t1;
    t2 = vec->t2;
    t3 = vec->t3;
    t4 = vec->t4;
  }
  
  // Copy Function (set existing to same as other):
  void copy(jvec4d* vec) {
    t1 = vec->t1;
    t2 = vec->t2;
    t3 = vec->t3;
    t4 = vec->t4;
  }
  
  double get(uint32_t i) {
    switch(i) {
      case 0: return t1;
      case 1: return t2;
      case 2: return t3;
      case 3: return t4;
      default: assert(0);
    }
  }
  
  void set(uint32_t i, double val) {
    switch(i) {
      case 0: t1 = val; return;
      case 1: t2 = val; return;
      case 2: t3 = val; return;
      case 3: t4 = val; return;
      default: assert(0);
    }
  }
  
  void set(double a_t1, double a_t2, double a_t3, double a_t4) {
    t1 = a_t1;
    t2 = a_t2;
    t3 = a_t3;
    t4 = a_t4;
  }

	void clear() {
		t1 = 0.0;
		t2 = 0.0;
		t3 = 0.0;
		t4 = 0.0;
	}	
  
  void print() {
    printf("(%.2f, %.2f, %.2f, %.2f)", 
           t1*180/M_PI, t2*180/M_PI, t3*180/M_PI, t4*180/M_PI);
  }
  
  void print_rad() {
    printf("(%.2f, %.2f, %.2f, %.2f)", 
           t1, t2, t3, t4);
  }
  
  void println() {
    print();
    printf("\n");
  }
};

struct RobotModel {
	double l1;
	double l2;
	double l3;
	double l4;
	double l5;
	double m1;
	double m2;
	double m3;
	double m4;
	double m5;
  

  double h1, h2, h3, h4, h5;    // Heights
  double w1, w2, w3, w4, w5;    // Widths 
  double c1, c2, c3, c4, c5;    // Centre of massesses

   

  void setLengths(double a_l1, double a_l2, double a_l3, double a_l4, double a_l5) {
    l1 = a_l1;
    l2 = a_l2;
    l3 = a_l3;
    l4 = a_l4;
    l5 = a_l5;
  }
  
  void setMasses(double a_m1, double a_m2, double a_m3, double a_m4, double a_m5) {
    m1 = a_m1;
    m2 = a_m2;
    m3 = a_m3;
    m4 = a_m4;
    m5 = a_m5;
  }

  void setHeights(double a_h1, double a_h2, double a_h3, double a_h4, double a_h5) {
    h1 = a_h1;
    h2 = a_h2;
    h3 = a_h3;
    h4 = a_h4;
    h5 = a_h5;
  }


  void setWidths(double a_w1, double a_w2, double a_w3, double a_w4, double a_w5) {
    w1 = a_w1;
    w2 = a_w2;
    w3 = a_w3;
    w4 = a_w4;
    w5 = a_w5;
  }

   void setCOM(double a_c1, double a_c2, double a_c3, double a_c4, double a_c5) {
    c1 = a_c1;
    c2 = a_c2;
    c3 = a_c3;
    c4 = a_c4;
    c5 = a_c5;
  } 

};

struct Setpoint {
  uint32_t time;   // milliseconds
  jvec4d* jpos;    // rad
  jvec4d* jvel;    // rad/s
  jvec4d* jacc;    // rad/s2
  cvec4d* cpos;    // m
  cvec4d* cvel;    // m/s
  cvec4d* cacc;    // m/s2
  
  Setpoint(uint64_t a_time, bool newvars) {
    time = a_time;
    if(newvars) {
      jpos = new jvec4d();
      jvel = new jvec4d();
      jacc = new jvec4d();
      cpos = new cvec4d();
      cvel = new cvec4d();
      cacc = new cvec4d();
    }
  }
  
  Setpoint(uint64_t a_time,
           jvec4d* a_jpos, jvec4d* a_jvel, jvec4d* a_jacc, 
           cvec4d* a_cpos, cvec4d* a_cvel, cvec4d* a_cacc) {
    time = a_time;
    jpos = new jvec4d(a_jpos);
    jvel = new jvec4d(a_jvel);
    jacc = new jvec4d(a_jacc);
    cpos = new cvec4d(a_cpos);
    cvel = new cvec4d(a_cvel);
    cacc = new cvec4d(a_cacc);
  }
  
  // Copy Constructor (calls all the other copy constructors):
  Setpoint(Setpoint* sp) {
    time = sp->time;
    jpos = new jvec4d(sp->jpos);
    jvel = new jvec4d(sp->jvel);
    jacc = new jvec4d(sp->jacc);
    cpos = new cvec4d(sp->cpos);
    cvel = new cvec4d(sp->cvel);
    cacc = new cvec4d(sp->cacc);
  }
  
  ~Setpoint() {
    delete jpos;
    delete jvel;
    delete jacc;
    delete cpos;
    delete cvel;
    delete cacc;
  }
  
  void copy(Setpoint* sp) {
    delete jpos;
    delete jvel;
    delete jacc;
    delete cpos;
    delete cvel;
    delete cacc;
    time = sp->time;
    jpos = new jvec4d(sp->jpos);
    jvel = new jvec4d(sp->jvel);
    jacc = new jvec4d(sp->jacc);
    cpos = new cvec4d(sp->cpos);
    cvel = new cvec4d(sp->cvel);
    cacc = new cvec4d(sp->cacc);
  }
  
  void print() {
    printf("t=%i: ", time);
    printf("jpos=");    jpos->print();
    printf(" jvel=");   jvel->print();
    printf(" jacc=");   jacc->println();
    printf("cpos=");    cpos->print();
    printf(" cvel=");   cvel->print();
    printf(" cacc=");   cacc->println();
  }
  
  void println() {
    print();
    printf("\n");
  }
};

#endif
